/*
* @File Name steppersIP.c
* @File Path M:\MAS2\PRM_Robotic_Arm\PRMCcode\coding\trajectory\stepper\stepperDriver\steppersIP.c
* @Author: Ruige_Lee
* @Date:   2019-04-22 17:09:51
* @Last Modified by:   Ruige_Lee
* @Last Modified time: 2019-05-08 20:26:08
* @Email: 295054118@whut.edu.cn
* @page: https://whutddk.github.io/
*//*
* @File Name steppersIP.c
* @File Path M:\MAS2\PRM_Robotic_Arm\PRMCcode\coding\trajectory\stepper\stepperDriver\steppersIP.c
* @Author: Ruige_Lee
* @Date:   2019-04-17 14:12:43
* @Last Modified by:   Ruige_Lee
* @Last Modified time: 2019-04-22 17:02:57
* @Email: 295054118@whut.edu.cn"
*/







#include <stdint.h>

#include "divLUT.h"
#include "steppersIP.h"



/*
pluseCnt : allow -1023 ~ 1023
*/

static inline int32_t load_div_lookUpTable( int16_t pluseCnt )
{
//	Assert((pluseCnt > -1023) && (pluseCnt < 1023));

	int32_t stepCnt;

	uint16_t absPluse;

	if ( pluseCnt > 255 )
	{
		pluseCnt = 255;
	}

	if ( pluseCnt < -255 )
	{
		pluseCnt = -255;
	}


	if ( pluseCnt > 0 )
	{
		absPluse = pluseCnt;
		stepCnt = divLUT[absPluse];
	}
	else
	{
		absPluse = -pluseCnt;
		stepCnt = -divLUT[absPluse];
	}


	return stepCnt;
}



/***************************** User  API *****************************************************/

uint32_t pl_stepper_param_write( uint8_t robot, uint8_t axis, int16_t pluseCnt )
{
	int32_t regValue = 0;

	UINTPTR addr = 0;

	uint8_t axisOffset;
	if ( robot == 0 )
	{
		axisOffset = axis;
	}
	else
	{
		axisOffset = axis + 6;
	}

	addr = AXI_STEPPER_BASE + axisOffset*4;
	regValue = load_div_lookUpTable( pluseCnt );

	Xil_Out32(addr, regValue);
	return 0;
}




uint32_t pl_stepper_param_read( uint8_t robot, uint8_t axis )
{
	UINTPTR addr = 0;

	uint8_t axisOffset;


	if ( robot == 0 )
	{
		axisOffset = axis + 12;
	}
	else
	{
		axisOffset = axis + 18;
	}

	addr = AXI_STEPPER_BASE + (axisOffset) * 4 ;

	return (Xil_In32(addr));
}


/******Assist Register********/

void pl_stepper_powerOn(uint8_t robot)
{
	if ( robot == 0 )
	{
		UINTPTR addr = AXI_STEPPER_BASE + STEPPERA_CFG_POWER_ON_OFFSET;
		Xil_Out32(addr, 0x00000001);
	}
	else
	{
		UINTPTR addr = AXI_STEPPER_BASE + STEPPERB_CFG_POWER_ON_OFFSET;
		Xil_Out32(addr, 0x00000001);
	}	

}

void pl_stepper_powerOff(uint8_t robot)
{
	if ( robot == 0 )
	{
		UINTPTR addr = AXI_STEPPER_BASE + STEPPERA_CFG_POWER_ON_OFFSET;
		Xil_Out32(addr, 0x00000000);
	}
	else
	{
		UINTPTR addr = AXI_STEPPER_BASE + STEPPERB_CFG_POWER_ON_OFFSET;
		Xil_Out32(addr, 0x00000000);
	}

}

void pl_stepper_safty_enable(uint8_t robot)
{
	if ( robot == 0 )
	{
		UINTPTR addr = AXI_STEPPER_BASE + STEPPERA_CFG_DRIVER_ENA_OFFSET;
		Xil_Out32(addr, 0x00000001);
	}
	else
	{
		UINTPTR addr = AXI_STEPPER_BASE + STEPPERB_CFG_DRIVER_ENA_OFFSET;
		Xil_Out32(addr, 0x00000001);
	}
}

void pl_stepper_safty_disable(uint8_t robot)
{
	if ( robot == 0 )
	{
		UINTPTR addr = AXI_STEPPER_BASE + STEPPERA_CFG_DRIVER_ENA_OFFSET;
		Xil_Out32(addr, 0x00000000);
	}
	else
	{
		UINTPTR addr = AXI_STEPPER_BASE + STEPPERB_CFG_DRIVER_ENA_OFFSET;
		Xil_Out32(addr, 0x00000000);
	}
}

void pl_stepper_reset()
{
	UINTPTR addr = AXI_STEPPER_BASE + STEPPERB_CFG_RESET_OFFSET;
	Xil_Out32(addr, 0x00000000);
}


void pl_stepper_set()
{
	UINTPTR addr = AXI_STEPPER_BASE + STEPPERB_CFG_RESET_OFFSET;
	Xil_Out32(addr, 0x00000001);
}
